/*******************************************************************************
 * File name: startup.S
 * Description: asembler file for LPC2478 startup
 * Project: _Wzorzec
 * Target: LPC2478
 * Compiler: arm-none-eabi-gcc
 * Date: 2010-10-12
 * Author: Kuba
 * Based on: Freddie Chopin, http://www.freddiechopin.info
 *******************************************************************************/

/*==============================================================================
 Includes
==============================================================================*/
#include "hdr_cpsr.h"

/*------------------------------------------------------------------------------
 ARM7 (ARM7TDMI-S) startup code
------------------------------------------------------------------------------*/
.text					/* section contains program code */
.balign 2				/* data alignment, 2 specyfies multiple of 2*/
.syntax unified			/* set the Instruction Set Syntax to correctly distinguish between ARM & THUMB */
.arm					/* selects the instruction set to 32 ARM. The same as .code 32 */
.func   Reset_Handler	/* emits debugging info for the Reset_Hanlder. Must terminate with .endfunct */
.global Reset_Handler	/* makes the symbol visible to ld- it's available to other partial programs */

/* Stack setup */
Reset_Handler:
	/*
	 * After reset the core is in Supervisor mode with IRQ & FIQ disabled
	 *
	 * The GNU toolset uses the full descending stack meaning that the stack
	 * grows towards the lower memory addresses
	 *
	 * ldr- load
	 * msr- move register to (C)PSR- (Current) Program Status Register
	*/
	ldr   sp, =__supervisor_stack_end;				/* set the stack for Supervisor mode, __supervisor_stack_end is defined in linker script */
	msr   CPSR_c, #FIQ_MODE | CPSR_I | CPSR_F		/* switch to FIQ mode, IRQ & FIQ disabled */
	ldr   sp, =__fiq_stack_end;						/* set the stack for FIQ mode, __fiq_stack_end is defined in linker script */
	msr   CPSR_c, #IRQ_MODE | CPSR_I | CPSR_F 		/* switch to IRQ mode, IRQ & FIQ disabled */
	ldr   sp, =__irq_stack_end;						/* set the stack for IRQ mode, __irq_stack_end is defined in linker script */
	msr   CPSR_c, #ABORT_MODE | CPSR_I | CPSR_F		/* switch to Abort mode, IRQ & FIQ disabled */
	ldr   sp, =__abort_stack_end;					/* set the stack for Abort mode, __abort_stack_end is defined in linker script */
	msr   CPSR_c, #UNDEFINED_MODE | CPSR_I | CPSR_F	/* switch to Undefined mode, IRQ & FIQ disabled */
	ldr   sp, =__undefined_stack_end;				/* set the stack for Undefined mode, __undefined_stack_end is defined in linker script */
	msr   CPSR_c, #SYSTEM_MODE | CPSR_I | CPSR_F	/* switch to System mode, IRQ & FIQ disabled */
	ldr   sp, =__user_system_stack_end;				/* set the stack for User and System mode, __user_system_stack_end is defined in linker script */

/* Branch to low_level_init_0() function- .data and .bss are not initialized! */
/* low_level_init must be compiled in arm mode (32 bits) and cannot relay on the
initialization of .data, .bss sections */
	ldr		r0, =low_level_init_0		/* r0 is loaded with the address of low_level_init_0 */
	mov		lr, pc						/* set the return address after the remap */
	bx		r0							/* invoke the low_level_init_0 */

/* Initialize .data section- relocate it by copying from ROM to RAM */
	ldr     r1, =__data_init_start
	ldr     r2, =__data_start
	ldr     r3, =__data_end
1:	cmp     r2, r3
	ldrlo   r0, [r1], #4
	strlo   r0, [r2], #4
	blo     1b

/* Zero-init .bss section */
	mov     r0, #0
	ldr     r1, =__bss_start
	ldr     r2, =__bss_end
1:	cmp     r1, r2
	strlo   r0, [r1], #4
	blo     1b

/* Call C++ constructors for global and static objects */
#ifdef __USES_CXX
	ldr		r0, =__libc_init_array	/* the library function __libc_init_array invokes all C++ static constructors */
	mov		lr, pc					/* set the return address */
	bx		r0						/* __libc_init_array is invoked here */
#endif

/* Branch to low_level_init_1() function */
	ldr		r0, =low_level_init_1
	mov		lr, pc
	bx		r0

/* Branch to main() with link */
	ldr		r0, =main				/* r0 is loaded with the main() address */
	mov		lr, pc
	bx		r0

/* Call C++ destructors for global and static objects */
#ifdef __USES_CXX
	ldr		r0, =__libc_fini_array
	mov		lr, pc
	bx		r0
#endif

/* On return - loop till the end of the world */
/* main() should not never return, because there is no OS to return to. In case
	main() ever returns it will encounter the endless loop */
	b		.

.endfunc /* end of Reset_Handler */

/*------------------------------------------------------------------------------
 __default_low_level_init() - replacement for undefined low_level_init_0()
 and/or low_level_init_1(). This function just returns.
------------------------------------------------------------------------------*/
.text
.balign 2
.syntax unified
.arm
.func   __default_low_level_init
.global __default_low_level_init

__default_low_level_init:
	bx		lr

.endfunc


/*------------------------------------------------------------------------------
 Assign undefined low_level_init_0() and/or low_level_init_1() to
 __default_low_level_init()
------------------------------------------------------------------------------*/
.weak	low_level_init_0 							/* sets the weak attribute- it causes the declaration to be emitted as a weak symbol rather than a global */
.global	low_level_init_0
.set	low_level_init_0, __default_low_level_init	/* set the low_level_init_0 as __default_low_level_init */

.weak	low_level_init_1
.global	low_level_init_1
.set	low_level_init_1, __default_low_level_init

/*------------------------------------------------------------------------------
 IRQ handler which allows to nest IRQ interrupts
------------------------------------------------------------------------------*/

.set VIC_BASE_ADDR,	0xFFFFF000
.set VICVectAddr, 	0xF00

.text
.balign 2
.syntax unified
.arm
.func IRQ_Handler
.global IRQ_Handler

IRQ_Handler:						/* LR points 2 instructions ahead of interrupted instruction */
	sub 	lr, 	lr, #4			/* by assigning PC to LR-4 the PC'll point to the next instruction after interruption */
	stmfd 	sp!, 	{lr}			/* (1) save LR_irq in IRQ stack */

	mrs 	r14,	SPSR			/* copy to R14 the SPSR register */
	stmfd	sp!,	{r14}			/* (2) and save it on the stack */

	stmfd 	sp!, 	{r0}			/* (3) R0 is used in next step- save it on the stack */

	ldr		r14, 	=VIC_BASE_ADDR	/* load the ISR address from Vectorized Interrupt Controller */
	ldr		r0, 	[r14, #VICVectAddr]

	msr		CPSR_c, #SYSTEM_MODE	/* switch to the System mode and enable Interrupts */

	stmfd	sp!,	{r0-r3, r12, r14}	/* (4) save context on System stack; r0-r3 and r12 are not preserved (APCS) */

	add		r14, 	pc, #4				/* before the branch, remember the return address */
	bx		r0							/* branch to the routine pointed by the VICVectAddr */

	ldmfd	sp!, 	{r0-r3, r12, r14}	/* (4) restore the context from the User stack */

	msr		CPSR_c,	#CPSR_I | IRQ_MODE	/* disable IRQs (FIQ enabled!) and get back to the IRQ mode */
										/* because on IRQ stack there are registers pushed */
	/* ldr	r14, 	=VIC_BASE_ADDR */	/* mark the end of interrupt */
	/* str	r14, [r14, #VICVectAddr] */

	ldmfd	sp!, 	{r0}				/* (3) restore r0 */

	ldmfd	sp!, 	{r14}				/* restore SPSR_irq and r0 from the IRQ stack */
	msr		SPSR_cxsf, r14				/* _cxsf is for a future compatibility */

	ldmfd	sp!, {pc}^					/* restore LR_irq from IRQ stack to PC, ^ means that CPSR'll be restored */

.endfunc	/* end of IRQ_Handler */

/*------------------------------------------------------------------------------
 SWI handler which allows to nest SWI interrupts
------------------------------------------------------------------------------*/

/*
coming repeatedly to the SWI_Nested_Handler from main
 the stack was changed by an offset 0x10 this is why
 the sp is increased at the beginning of the handler.
To sumarize:
1. set the stack by increasing sp
2. push r0, r11, lr, which are used here
3. push SPSR
4. to get back from swiIsr with unchanged stack save it in r11
5. find out what the swi parameter is
6. restore correct sp
7. begin popping SPSR, r0, r11, lr
8. get the hell out of here
*/

/*
.text
.balign 2
.syntax unified
.arm
.func SWI_Nested_Handler
.global SWI_Nested_Handler

SWI_Nested_Handler:
	add		sp,		sp, #16
	stmfd	sp!, 	{r0, r11, lr}			// APCS registers stacked

	mrs		r0,		SPSR					// get SPSR
	stmfd	sp!,	{r0}					// and stack it

	add 	r11,	sp, #0

	ldr 	r0, 	[lr, #-4]
	bic		r0, 	r0, #0xFF000000

	bl		swiHandler

	sub		sp,		r11, #0
	ldmfd	sp!,	{r0}
	msr		SPSR_cxsf,	r0					// restore SPSR
	ldmfd 	sp!,	{r0, r11, pc}^
.endfunc */	/* end of SWI_Nested_Handler */

/******************************************************************************
* END OF FILE
******************************************************************************/
